#include "pwm.h"


u16 LEDS = GPIO_Pin_12 | GPIO_Pin_13 | GPIO_Pin_14 | GPIO_Pin_15;
u16 LED1 = GPIO_Pin_12;
u16 LED2 = GPIO_Pin_13;
u16 LED3 = GPIO_Pin_14;
u16 LED4 = GPIO_Pin_15;
u32 LIS302DL_FLAG_TIMEOUT = 0x1000;


//in is a value between 0 and 100. out is a value between 1580 and 3160
int PWMConversion(int in)
{
	//double offset = 1580.0;
	//double max = 3160.0;

	float offset = 400.0;
	float max = 1900.0;

	float dif = max-offset;
	float tick = dif/1000.0;

	return (tick*((float)in));
}

//in is a value between 0 and 100. out is a value between 1580 and 3160
int PWMConversionOffZero(int in)
{
	//double offset = 1580.0;
	//double max = 3160.0;

	float offset = 400.0;
	float max = 1900.0;

	float dif = max-offset;
	float center = (dif/2.0f)+offset;
	float tick = dif/1000.0;

	return center+(tick*((float)in));
}


void UpdatePWMOutput(struct FlightDataStruct* state)
{

	/*
	* Servo min(1ms) = 1580
	* 2370 = 1.5ms - for Servo
	* Servo max(2ms) = 3160
	*
	*/

	double brightness =
			sqrt((state->current_Accel1_X*state->current_Accel1_X)+
				(state->current_Accel1_Y*state->current_Accel1_Y)+
				(state->current_Accel1_Z*state->current_Accel1_Z));

	//TIM4->CCR1=(brightness-1.0)*1;
	//TIM4->CCR2=(brightness-1.0)*10;
	//TIM4->CCR3=(brightness-1.0)*100;
	//TIM4->CCR4=(brightness-1.0)*1000;

	//TIM_SetCompare1(TIM4, state->current_Mag1_Y);
	//TIM_SetCompare2(TIM4, state->current_Mag1_X);
	//TIM_SetCompare3(TIM4, -state->current_Mag1_Y);
	//TIM_SetCompare4(TIM4, -state->current_Mag1_X);
	//TIM_SetCompare1(TIM4, state->tog);
	//TIM_SetCompare2(TIM4, state->tog*2);
	//TIM_SetCompare3(TIM4, state->tog*4);
	//TIM_SetCompare4(TIM4, state->tog*8);
	//TIM_SetCompare1(TIM4, 1580);
	//TIM_SetCompare2(TIM4, 2370);
	//TIM_SetCompare3(TIM4, 3160);
	//TIM_SetCompare4(TIM4, state->movement);

	//TIM_SetCompare2(TIM3, PWMPercentConversion(brightness));


	int i = 0;
	double mul = 0;
	int out = 0;


//TIM_SetCompare2(TIM3, PWMConversionOffZero(state->yawaverage));//for yaw control
TIM_SetCompare2(TIM3, PWMConversionOffZero(state->roll*100000000.0f));


	/*
for(i = 400;i<1900;i++)
{
	int z=0;
		for(z=0;z<100000;z++);
		//mul = sin((i/100.0));
	TIM_SetCompare2(TIM3, i);
}
*/



}


void init_PWM() {
    TIM_OCInitTypeDef TIM_OCInitStructure;


    TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
    TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
    TIM_OCInitStructure.TIM_Pulse = 0;
    TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;

    /* PWM1 Mode configuration: Channel1 (GPIOD Pin 12)*/
   // TIM_OC1Init(TIM4, &TIM_OCInitStructure);
  //  TIM_OC1PreloadConfig(TIM4, TIM_OCPreload_Enable);

    /* PWM1 Mode configuration: Channel2 (GPIOD Pin 13)*/
  //  TIM_OC2Init(TIM4, &TIM_OCInitStructure);
 //   TIM_OC2PreloadConfig(TIM4, TIM_OCPreload_Enable);

    /* PWM1 Mode configuration: Channel3 (GPIOD Pin 14)*/
  //  TIM_OC3Init(TIM4, &TIM_OCInitStructure);
  //  TIM_OC3PreloadConfig(TIM4, TIM_OCPreload_Enable);

    /* PWM1 Mode configuration: Channel4 (GPIOD Pin 15)*/
 //   TIM_OC4Init(TIM4, &TIM_OCInitStructure);
 //   TIM_OC4PreloadConfig(TIM4, TIM_OCPreload_Enable);

    /* PWM1 Mode configuration: Channel2 (GPIOB Pin 5)*/
    TIM_OC2Init(TIM3, &TIM_OCInitStructure);
    TIM_OC2PreloadConfig(TIM3, TIM_OCPreload_Enable);


    // Enable Timer Interrupt and Timer
    //TIM_ITConfig(TIM4, TIM_IT_Update, ENABLE); //
    TIM_ITConfig(TIM3, TIM_IT_Update, ENABLE); //
    TIM_Cmd(TIM3, ENABLE);
}

void initLeds() {
    GPIO_InitTypeDef GPIO_InitStructure;
    GPIO_InitTypeDef GPIO_InitStruct;

  //  RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOD, ENABLE);
    RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE);

/*
    GPIO_InitStructure.GPIO_Pin = LEDS;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;
    GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
    GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
    GPIO_Init(GPIOD, &GPIO_InitStructure);
    */
/*	GPIO_InitStruct.GPIO_Pin = GPIO_Pin_15 | GPIO_Pin_14 | GPIO_Pin_13 | GPIO_Pin_12; // we want to configure all LED GPIO pins
	GPIO_InitStruct.GPIO_Mode = GPIO_Mode_OUT; 		// we want the pins to be an output
	GPIO_InitStruct.GPIO_Speed = GPIO_Speed_100MHz; 	// this sets the GPIO modules clock speed
	GPIO_InitStruct.GPIO_OType = GPIO_OType_PP; 	// this sets the pin type to push / pull (as opposed to open drain)
	GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_NOPULL; 	// this sets the pullup / pulldown resistors to be inactive
	GPIO_Init(GPIOD, &GPIO_InitStruct);
*/
	GPIO_InitStruct.GPIO_Pin = GPIO_Pin_5; // we want to configure all LED GPIO pins
	GPIO_InitStruct.GPIO_Mode = GPIO_Mode_AF; 		// we want the pins to be an output
	GPIO_InitStruct.GPIO_Speed = GPIO_Speed_100MHz; 	// this sets the GPIO modules clock speed
	GPIO_InitStruct.GPIO_OType = GPIO_OType_PP; 	// this sets the pin type to push / pull (as opposed to open drain)
	GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_UP; 	// this sets the pullup / pulldown resistors to be inactive
	GPIO_Init(GPIOB, &GPIO_InitStruct);

	GPIO_PinAFConfig(GPIOB, GPIO_PinSource5, GPIO_AF_TIM3);
/*
    GPIO_PinAFConfig(GPIOD, GPIO_PinSource12, GPIO_AF_TIM4);
    GPIO_PinAFConfig(GPIOD, GPIO_PinSource13, GPIO_AF_TIM4);
    GPIO_PinAFConfig(GPIOD, GPIO_PinSource14, GPIO_AF_TIM4);
    GPIO_PinAFConfig(GPIOD, GPIO_PinSource15, GPIO_AF_TIM4);*/
}


void initTimer() {
    /* TIM4 clock enable */
//    RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE);

    TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
/*    TIM_TimeBaseStructInit (&TIM_TimeBaseStructure);

    TIM_TimeBaseStructure.TIM_Prescaler = SystemCoreClock/5000000; // pers 33.6
    TIM_TimeBaseStructure.TIM_Period = 31600; // 20ms for servo period

    TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
    TIM_TimeBaseInit(TIM4 , &TIM_TimeBaseStructure);

*/
    RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE);

    TIM_TimeBaseStructInit (&TIM_TimeBaseStructure);

    TIM_TimeBaseStructure.TIM_Prescaler = SystemCoreClock/5000000; // pers 33.6
    TIM_TimeBaseStructure.TIM_Period = 3160; // 20ms for servo period

    TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
    TIM_TimeBaseInit(TIM3 , &TIM_TimeBaseStructure);

}




